﻿using System;
using System.Collections.Generic;
using System.Globalization;
using MetadataExtractor;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using MetadataExtractor.Formats.Xmp;
using System.Linq;

public class PhotoGeotagCalculator
{
    // 无人机相机-天线偏移参数 (单位:米)
    public struct AntennaOffset
    {
        public double DeltaX; // 前向偏移
        public double DeltaY; // 右向偏移
        public double DeltaZ; // 下向偏移
    }

    // 无人机姿态数据
    public struct DroneAttitude
    {
        public double Yaw;     // 偏航角 (度)
        public double Pitch;   // 俯仰角 (度)
        public double Roll;    // 横滚角 (度)
    }

    // 高精度坐标结果
    public struct GeoCoordinate
    {
        public double Latitude { get; set; }  // 纬度 (度)
        public double Longitude { get; set; } // 经度 (度)
        public double Altitude { get; set; }  // 椭球高 (米)
    }

    // 主要计算函数
    public static GeoCoordinate CalculatePhotoCenter(
        string imagePath, 
        AntennaOffset offset,
        GeoCoordinate? baseStation = null,
        Dictionary<DateTime, GeoCoordinate> ppkData = null)
    {
        // 1. 提取照片EXIF元数据
        var exifData = ExtractExifData(imagePath);
        
        // 2. 获取高精度位置 (优先使用PPK数据)
        GeoCoordinate antennaPos;
        if (ppkData != null && ppkData.TryGetValue(exifData.AcquisitionTime, out var ppkPos))
        {
            antennaPos = ppkPos;
        }
        else
        {
            // 使用EXIF中的GPS数据
            antennaPos = exifData.GpsPosition;
            
            // 若有基站数据，应用RTK校正
            if (baseStation.HasValue)
            {
                antennaPos = ApplyRtkCorrection(antennaPos, baseStation.Value);
            }
        }
        
        // 3. 计算相机中心位置
        var cameraCenter = CalculateCameraPosition(
            antennaPos, 
            exifData.Attitude,
            offset,
            exifData.RelativeAltitude);
        
        return cameraCenter;
    }

    // 提取EXIF元数据
    private static ExifData ExtractExifData(string imagePath)
    {
        var exifData = new ExifData();
        var directories = ImageMetadataReader.ReadMetadata(imagePath);
        string gpsLngTagDesc = null;
        string gpsLngTagRef = null;
        string gpsLatTagDesc = null;
        string gpsLatTagRef = null;
        foreach (var directory in directories)
        {            
            foreach (var tag in directory.Tags)
            {
                Console.WriteLine(directory.Name + " ========> " + tag.Name + " ========> " + tag.Description);
                switch (tag.Name)
                {
                    case "GPS Latitude":
                        gpsLatTagDesc = tag.Description;
                        //exifData.GpsPosition.Latitude = ParseGpsCoordinate(tag.Description);
                        break;
                    case "GPS Latitude Ref":
                        gpsLatTagRef = tag.Description;
                        break;
                    case "GPS Longitude":
                        gpsLngTagDesc = tag.Description;
                        //exifData.GpsPosition.Longitude = ParseGpsCoordinate(tag.Description);
                        break;
                    case "GPS Longitude Ref":
                        gpsLngTagRef = tag.Description;
                        break;
                    case "GPS Altitude":
                        exifData.GpsPosition.Altitude = double.Parse(tag.Description.Split(' ')[0]);
                        break;
                    case "Flight Yaw Degree":
                        exifData.Attitude.Yaw = double.Parse(tag.Description);
                        break;
                    case "Gimbal Yaw Degree":
                        exifData.Attitude.Yaw += double.Parse(tag.Description); // 组合偏航角
                        break;
                    case "Gimbal Pitch Degree":
                        exifData.Attitude.Pitch = double.Parse(tag.Description);
                        break;
                    case "Gimbal Roll Degree":
                        exifData.Attitude.Roll = double.Parse(tag.Description);
                        break;
                    case "Relative Altitude":
                        exifData.RelativeAltitude = double.Parse(tag.Description.Split(' ')[0]);
                        break;
                    case "Date/Time Original":
                        exifData.AcquisitionTime = DateTime.ParseExact(
                            tag.Description, "yyyy:MM:dd HH:mm:ss", CultureInfo.InvariantCulture);
                        break;
                }
            }
        }
        var xmpDirectories = directories.OfType<XmpDirectory>();
        // 遍历所有XMP目录并打印信息  
        foreach (var xmpDir in xmpDirectories)
        {
            Console.WriteLine("=== XMP元数据 ===");
            // 获取原始XMP数据（XML格式）  
            Console.WriteLine("\n[原始XMP XML数据]");
            Console.WriteLine(xmpDir.XmpMeta?.ToString() ?? "无原始数据");

            // 打印解析后的XMP标签  
            Console.WriteLine("\n[解析后的XMP标签]");
            foreach (var tag in xmpDir.XmpMeta.Properties)
            {
                // 输出：标签名称（命名空间）= 值  
                Console.WriteLine($"---------------------------------------------{tag.Path} ({tag.Path}): {tag.Value}");
                if (tag.Path == "drone-dji:DroneModel")
                {

                }
                if (tag.Path == "drone-dji:DroneSerialNumber")
                {

                }
                if (tag.Path == "drone-dji:CameraSerialNumber")
                {

                }
                if (tag.Path == "drone-dji:AbsoluteAltitude")
                {

                }
                if (tag.Path == "drone-dji:FlightPitchDegree")
                {

                }
                if (tag.Path == "drone-dji:FlightRollDegree")
                {

                }
                if (tag.Path == "drone-dji:FlightYawDegree")
                {

                }
                if (tag.Path == "drone-dji:GimbalPitchDegree")
                {

                }
                if (tag.Path == "drone-dji:GimbalRollDegree")
                {

                }
                if (tag.Path == "drone-dji:GimbalYawDegree")
                {

                }
                if (tag.Path == "drone-dji:GpsLatitude")
                {

                }
                if (tag.Path == "drone-dji:GpsLongitude")
                {

                }
                if (tag.Path == "drone-dji:AbsoluteAltitude")
                {

                }
            }
        }
        if (double.IsNaN(exifData.GpsPosition.Longitude) && !string.IsNullOrEmpty(gpsLngTagDesc))
        {
            exifData.GpsPosition.Longitude = ParseGpsCoordinate(gpsLngTagDesc, gpsLngTagRef);
        }
        if(double.IsNaN(exifData.GpsPosition.Latitude) && !string.IsNullOrEmpty(gpsLatTagDesc))
        {
            exifData.GpsPosition.Latitude = ParseGpsCoordinate(gpsLatTagDesc, gpsLatTagRef);
        }
        
        
        return exifData;
    }

    // 解析GPS坐标 (格式: "40° 42' 51.6\" N")
    private static double ParseGpsCoordinate(string gpsString, string gpsRef = null)
    {
        var parts = gpsString.Split(new[] { '°', '\'', '\"' }, StringSplitOptions.RemoveEmptyEntries);
        double degrees = double.Parse(parts[0]);
        double minutes = double.Parse(parts[1]);
        double seconds = double.Parse(parts[2]);
        double sign = 1;
        if(string.IsNullOrEmpty(gpsRef))
        {
            if(parts.Length >= 3)
            {
                sign = parts[3].Trim() == "N" || parts[3].Trim() == "E" ? 1 : -1;
            }
        } 
        else
        {
            if("N" != gpsRef.Trim() && "E" != gpsRef.Trim())
            {
                sign = -1;
            }
        }
        
        
        return sign * (degrees + minutes / 60 + seconds / 3600);
    }

    // RTK校正 (简化实现)
    private static GeoCoordinate ApplyRtkCorrection(
        GeoCoordinate rover, 
        GeoCoordinate baseStation)
    {
        // 实际应用中应使用RTK算法，此处为简化示例
        return new GeoCoordinate
        {
            Latitude = rover.Latitude + (rover.Latitude - baseStation.Latitude) * 0.000001,
            Longitude = rover.Longitude + (rover.Longitude - baseStation.Longitude) * 0.000001,
            Altitude = rover.Altitude + (rover.Altitude - baseStation.Altitude) * 0.1
        };
    }

    // 核心计算：从天线位置计算相机中心位置
    private static GeoCoordinate CalculateCameraPosition(
        GeoCoordinate antennaPos,
        DroneAttitude attitude,
        AntennaOffset offset,
        double relativeAltitude)
    {
        // 1. 将WGS84坐标转换为ECEF坐标
        var ecefAntenna = Wgs84ToEcef(antennaPos);
        
        // 2. 计算旋转矩阵
        var rotationMatrix = CalculateRotationMatrix(attitude);
        
        // 3. 计算天线到相机的偏移向量 (在机体坐标系)
        var bodyOffset = Vector<double>.Build.Dense(new[] 
        { 
            offset.DeltaX, 
            offset.DeltaY, 
            offset.DeltaZ 
        });
        
        // 4. 将偏移向量旋转到ECEF坐标系
        var ecefOffset = rotationMatrix * bodyOffset;
        
        // 5. 计算相机的ECEF坐标
        var ecefCamera = ecefAntenna + ecefOffset;
        
        // 6. 将ECEF坐标转回WGS84
        var cameraPos = EcefToWgs84(ecefCamera);
        
        // 7. 高度修正 (使用相对高度)
        cameraPos.Altitude = antennaPos.Altitude + relativeAltitude;
        
        return cameraPos;
    }

    // 计算旋转矩阵 (机体坐标系到ECEF)
    private static Matrix<double> CalculateRotationMatrix(DroneAttitude attitude)
    {
        // 转换为弧度
        double yaw = DegreesToRadians(attitude.Yaw);
        double pitch = DegreesToRadians(attitude.Pitch);
        double roll = DegreesToRadians(attitude.Roll);
        
        // 计算旋转矩阵 (Z-Y-X顺序)
        Matrix<double> Rz = DenseMatrix.OfArray(new[,]
        {
            { Math.Cos(yaw), -Math.Sin(yaw), 0 },
            { Math.Sin(yaw), Math.Cos(yaw), 0 },
            { 0, 0, 1 }
        });
        
        Matrix<double> Ry = DenseMatrix.OfArray(new[,]
        {
            { Math.Cos(pitch), 0, Math.Sin(pitch) },
            { 0, 1, 0 },
            { -Math.Sin(pitch), 0, Math.Cos(pitch) }
        });
        
        Matrix<double> Rx = DenseMatrix.OfArray(new[,]
        {
            { 1, 0, 0 },
            { 0, Math.Cos(roll), -Math.Sin(roll) },
            { 0, Math.Sin(roll), Math.Cos(roll) }
        });
        
        return Rz * Ry * Rx;
    }

    // WGS84转ECEF (单位:米)
    private static Vector<double> Wgs84ToEcef(GeoCoordinate geo)
    {
        const double a = 6378137.0; // WGS84长半轴
        const double e2 = 6.6943799901377997e-3; // 第一偏心率平方
        
        double lat = DegreesToRadians(geo.Latitude);
        double lon = DegreesToRadians(geo.Longitude);
        double alt = geo.Altitude;
        
        double N = a / Math.Sqrt(1 - e2 * Math.Sin(lat) * Math.Sin(lat));
        
        double x = (N + alt) * Math.Cos(lat) * Math.Cos(lon);
        double y = (N + alt) * Math.Cos(lat) * Math.Sin(lon);
        double z = (N * (1 - e2) + alt) * Math.Sin(lat);
        
        return Vector<double>.Build.Dense(new[] { x, y, z });
    }

    // ECEF转WGS84
    private static GeoCoordinate EcefToWgs84(Vector<double> ecef)
    {
        const double a = 6378137.0;
        const double e2 = 6.6943799901377997e-3;
        const double f = 1 / 298.257223563;
        
        double x = ecef[0];
        double y = ecef[1];
        double z = ecef[2];
        
        double lon = Math.Atan2(y, x);
        double p = Math.Sqrt(x * x + y * y);
        double lat = Math.Atan2(z, p * (1 - e2));
        
        // 迭代计算纬度
        for (int i = 0; i < 10; i++)
        {
            double N = a / Math.Sqrt(1 - e2 * Math.Sin(lat) * Math.Sin(lat));
            double alt1 = p / Math.Cos(lat) - N;
            double latNew = Math.Atan2(z, p * (1 - e2 * N / (N + alt1)));
            
            if (Math.Abs(latNew - lat) < 1e-12)
                break;
            
            lat = latNew;
        }
        
        double N_final = a / Math.Sqrt(1 - e2 * Math.Sin(lat) * Math.Sin(lat));
        double alt = p / Math.Cos(lat) - N_final;
        
        return new GeoCoordinate
        {
            Latitude = RadiansToDegrees(lat),
            Longitude = RadiansToDegrees(lon),
            Altitude = alt
        };
    }

    // 度转弧度
    private static double DegreesToRadians(double degrees)
        => degrees * Math.PI / 180.0;
    
    // 弧度转度
    private static double RadiansToDegrees(double radians)
        => radians * 180.0 / Math.PI;

    // EXIF数据结构
    private struct ExifData
    {
        public GeoCoordinate GpsPosition;
        public DroneAttitude Attitude;
        public double RelativeAltitude;
        public DateTime AcquisitionTime;
    }
}

// 使用示例
public class PhotoGeotagCalculatorProgram
{
    public static void LMain()
    {
        // 配置无人机型号参数
        var offset = new PhotoGeotagCalculator.AntennaOffset
        {
            DeltaX = 0.20, // M300 RTK前向偏移
            DeltaY = 0.00, // 右侧偏移
            DeltaZ = -0.15 // 下向偏移 (负值表示向上)
        };
        offset = new PhotoGeotagCalculator.AntennaOffset
        {
            DeltaX = 0, // M300 RTK前向偏移
            DeltaY = 0.00, // 右侧偏移
            DeltaZ = 0 // 下向偏移 (负值表示向上)
        };

        // 基站坐标 (可选)
        var baseStation = new PhotoGeotagCalculator.GeoCoordinate
        {
            Latitude = 31.2304,
            Longitude = 121.4737,
            Altitude = 5.2
        };
        
        // PPK数据 (可选)
        var ppkData = new Dictionary<DateTime, PhotoGeotagCalculator.GeoCoordinate>
        {
            {
                DateTime.Parse("2023-05-15 10:30:25"),
                new PhotoGeotagCalculator.GeoCoordinate
                {
                    Latitude = 31.230416,
                    Longitude = 121.473702,
                    Altitude = 102.567
                }
            }
        };
        
        // 计算照片中心点
        var result = PhotoGeotagCalculator.CalculatePhotoCenter(
            @"D:\downloads\baidu\M4T\DJI_20250713144931_0003_T_HL01-345.JPG",
            offset);
        
        Console.WriteLine($"照片中心点坐标:");
        Console.WriteLine($"纬度: {result.Latitude:F8}°");
        Console.WriteLine($"经度: {result.Longitude:F8}°");
        Console.WriteLine($"海拔: {result.Altitude:F2} 米");
    }
}